﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Practice2 : MonoBehaviour
{
    [Serializable]
    public class Triangle
    {
        public Transform p0;
        public Transform p1;
        public Transform p2;
    }

    public Triangle aTriangle;
    public Triangle bTriangle;


    void OnDrawGizmos()
    {
        if (aTriangle.p0 == null) return;
        if (aTriangle.p1 == null) return;
        if (aTriangle.p2 == null) return;

        if (bTriangle.p0 == null) return;
        if (bTriangle.p1 == null) return;
        if (bTriangle.p2 == null) return;

        var cacheColor = Gizmos.color;

        var r = IsTriangleIntersect(
            aTriangle.p0.position
            , aTriangle.p1.position
            , aTriangle.p2.position
            , bTriangle.p0.position
            , bTriangle.p1.position
            , bTriangle.p2.position);

        Gizmos.color = r == null ? Color.white : Color.red;

        Gizmos.DrawLine(aTriangle.p0.position, aTriangle.p1.position);
        Gizmos.DrawLine(aTriangle.p1.position, aTriangle.p2.position);
        Gizmos.DrawLine(aTriangle.p2.position, aTriangle.p0.position);

        Gizmos.DrawLine(bTriangle.p0.position, bTriangle.p1.position);
        Gizmos.DrawLine(bTriangle.p1.position, bTriangle.p2.position);
        Gizmos.DrawLine(bTriangle.p2.position, bTriangle.p0.position);

        Gizmos.color = cacheColor;
    }

    Vector3? IsTriangleIntersect(Vector3 tri1A, Vector3 tri1B, Vector3 tri1C, Vector3 tri2A, Vector3 tri2B, Vector3 tri2C)
    {
        var r = IsTriangleIntersect(tri1A, tri1B, tri1C, tri2A, tri2B);
        if (r != null) return r;

        r = IsTriangleIntersect(tri1A, tri1B, tri1C, tri2B, tri2C);
        if (r != null) return r;

        r = IsTriangleIntersect(tri1A, tri1B, tri1C, tri2C, tri2A);
        if (r != null) return r;

        return null;
    }

    Vector3? IsTriangleIntersect(Vector3 triP0, Vector3 triP1, Vector3 triP2, Vector3 segP0, Vector3 segP1)
    {
        var result = default(Vector3?);
        var v1 = triP1 - triP0;
        var v2 = triP2 - triP0;
        var normal = Vector3.Cross(v1, v2);
        var center = (triP0 + triP1 + triP2) / 3;

        var intersectPoint = GetIntersectPoint(normal, center, segP0, segP1);

        if (intersectPoint != null)
        {
            Gizmos.DrawWireSphere(intersectPoint.Value, 0.1f);

            result = intersectPoint;

            var quat = Quaternion.FromToRotation(normal, Vector3.forward);
            var q1 = Vector3.ProjectOnPlane(quat * segP0, Vector3.forward);
            var q2 = Vector3.ProjectOnPlane(quat * segP1, Vector3.forward);
            intersectPoint = Vector3.ProjectOnPlane(quat * intersectPoint.Value, Vector3.forward);
            triP0 = Vector3.ProjectOnPlane(quat * triP0, Vector3.forward);
            triP1 = Vector3.ProjectOnPlane(quat * triP1, Vector3.forward);
            triP2 = Vector3.ProjectOnPlane(quat * triP2, Vector3.forward);

            var flag1 = !Is2DTriangleIntersect(q1, triP0, triP1, triP2);
            flag1 &= !Is2DTriangleIntersect(q2, triP0, triP1, triP2);

            var flag2 = Is2DTriangleIntersect(intersectPoint.Value, triP0, triP1, triP2);

            var flag = !flag2 || flag1;

            if (flag)
            {
                result = null;
            }
        }

        return result;
    }

    bool Is2DTriangleIntersect(Vector3 comparePoint, Vector3 triP0, Vector3 triP1, Vector3 triP2)
    {
        const float ERROR = 0.01f;

        var dir1 = triP1 - triP0;
        var dir2 = triP2 - triP1;
        var dir3 = triP0 - triP2;

        var cross1 = Mathf.Sign(Vector3.Cross(dir1, comparePoint - triP0).z);
        var cross2 = Mathf.Sign(Vector3.Cross(dir2, comparePoint - triP1).z);
        var cross3 = Mathf.Sign(Vector3.Cross(dir3, comparePoint - triP2).z);

        var dir = cross1 == cross2 ? cross3 : cross1 == cross3 ? cross2 : cross1;

        cross1 = Mathf.Sign(Vector3.Cross(dir1, comparePoint - triP0).z + -dir * ERROR);
        cross2 = Mathf.Sign(Vector3.Cross(dir2, comparePoint - triP1).z + -dir * ERROR);
        cross3 = Mathf.Sign(Vector3.Cross(dir3, comparePoint - triP2).z + -dir * ERROR);

        return Mathf.Approximately(cross1, cross2) && Mathf.Approximately(cross2, cross3);
    }

    Vector3? GetIntersectPoint(Vector3 planeNormal, Vector3 planePosition, Vector3 p0, Vector3 p1)
    {
        var sign1 = Mathf.Sign(Vector3.Dot(planeNormal, (planePosition - p0).normalized));
        var sign2 = Mathf.Sign(Vector3.Dot(planeNormal, (planePosition - p1).normalized));
        if (sign1 == sign2) return null;

        var a = planeNormal.x;
        var b = planeNormal.y;
        var c = planeNormal.z;
        var d = -a * planePosition.x - b * planePosition.y - c * planePosition.z;

        var i0 = a * p0.x + b * p0.y + c * p0.z;
        var i1 = a * p1.x + b * p1.y + c * p1.z;
        var final_t = -(i1 + d) / (i0 - i1);

        var finalPoint = new Vector3()
        {
            x = p0.x * final_t + p1.x * (1 - final_t),
            y = p0.y * final_t + p1.y * (1 - final_t),
            z = p0.z * final_t + p1.z * (1 - final_t),
        };

        return finalPoint;
    }
}
